#include "Core/FreeFlyCamera.h"
#include "Core/Device.h"
#include "Core/Renderer.h"
#include "Core/EventManager.h"
#include "GL/glfw.h"
#include <cmath>

namespace blue {

FreeFlyCamera::FreeFlyCamera(const Vector3d & position)
{
    _screenSize = Vector2i(Device::instance()->getSize().X, Device::instance()->getSize().Y);

    _position = position;
    _phi = 0;
    _theta = 0;

    _now = 0;
    _start_time = 0;
    _time_step = 0;

    VectorsFromAngles();

    _speed = 1.0;
    _sensivity = 100.0;

    _keymap["forward"] = 'Z';
    _keymap["backward"] = 'S';
    _keymap["strafe_left"] = 'Q';
    _keymap["strafe_right"] = 'D';
    _keymap["boost"] = GLFW_KEY_LSHIFT;

    _keystates["forward"] = false;
    _keystates["backward"] = false;
    _keystates["strafe_left"] = false;
    _keystates["strafe_right"] = false;
    _keystates["boost"] = false;

    resetCamera();
}

FreeFlyCamera::~FreeFlyCamera()
{
}

void FreeFlyCamera::resetCamera()
{
    _forward = Vector3d(0.0, 1.0, 0.0);
    _phi = 0;
    _theta = 0;
}

void FreeFlyCamera::setKeyMap(int forward, int backward, int left, int right, int boost)
{
    _keymap["forward"] = forward;
    _keymap["backward"] = backward;
    _keymap["strafe_left"] = left;
    _keymap["strafe_right"] = right;
    _keymap["boost"] = boost;
}

void FreeFlyCamera::onEvent()
{
    _keystates["forward"] = EventManager::instance()->isKeyDown(_keymap["forward"]);
    _keystates["backward"] = EventManager::instance()->isKeyDown(_keymap["backward"]);
    _keystates["strafe_left"] = EventManager::instance()->isKeyDown(_keymap["strafe_left"]);
    _keystates["strafe_right"] = EventManager::instance()->isKeyDown(_keymap["strafe_right"]);
    _keystates["boost"] = EventManager::instance()->isKeyDown(_keymap["boost"]);

    _theta -= EventManager::instance()->getRelMouseX()*_sensivity;
    _phi -= EventManager::instance()->getRelMouseY()*_sensivity;

    VectorsFromAngles();
}

void FreeFlyCamera::animate()
{
    _now = glfwGetTime();
    _time_step = _now - _start_time;
    _start_time = _now;

    double realspeed = (_keystates["boost"])?10*_speed:_speed;
    if (_keystates["forward"])
        _position += _forward * (realspeed * _time_step);
    if (_keystates["backward"])
        _position -= _forward * (realspeed * _time_step);
    if (_keystates["strafe_left"])
        _position += _left * (realspeed * _time_step);
    if (_keystates["strafe_right"])
        _position -= _left * (realspeed * _time_step);

    _target = _position + _forward;
}

void FreeFlyCamera::setSpeed(double speed)
{
    _speed = speed;
}

void FreeFlyCamera::setSensivity(double sensivity)
{
    _sensivity = sensivity;
}

void FreeFlyCamera::setPosition(const Vector3d & position)
{
    _position = position;
    _target = _position + _forward;
}

Vector3d FreeFlyCamera::getPosition() const
{
    return _position;
}

void FreeFlyCamera::look()
{
    Renderer::instance()->lookAt(_position, _target);
}

void FreeFlyCamera::VectorsFromAngles()
{
    static const Vector3d up(0,0,1);
    if (_phi > 89)
        _phi = 89;
    else if (_phi < -89)
        _phi = -89;
    double r_temp = cos(_phi*M_PI/180);

    _forward.Z = sin(_phi*M_PI/180);
    _forward.X = r_temp*cos(_theta*M_PI/180);
    _forward.Y = r_temp*sin(_theta*M_PI/180);

    _left = up.crossProduct(_forward);
    _left.normalize();

    _target = _position + _forward;
}

}
